"""
文件说明：USB2CANFD双通道CAN收发测试
更多帮助：www.toomoss.com
"""
from ctypes import *
import platform
from time import sleep
import datetime
import time
from usb_device import *
from usb2canfd import *

def CANFD_PrintErrorMsg(CANFDMsg):
    if CANFDMsg.ID==0xFFFFFFFF :
        BusStatus = CANFD_DIAGNOSTIC()
        memmove(byref(BusStatus),byref(CANFDMsg.Data),8)
        if(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_NBIT0_ERR|CANFD_DIAGNOSTIC_FLAG_DBIT0_ERR)):
            print("发送显性电平，但监视的总线值为隐性")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_NBIT1_ERR|CANFD_DIAGNOSTIC_FLAG_DBIT1_ERR)):
            print("发送隐性电平，但监视的总线值为显性")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_NACK_ERR)):
            print("发送报文未应答")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_NFORM_ERR|CANFD_DIAGNOSTIC_FLAG_DFORM_ERR)):
            print("接收报文的固定格式部分格式错误")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_NCRC_ERR|CANFD_DIAGNOSTIC_FLAG_DCRC_ERR)):
            print("CRC错误")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_NSTUFF_ERR|CANFD_DIAGNOSTIC_FLAG_DSTUFF_ERR)):
            print("位填充错误")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_TXBO_ERR)):
            print("器件进入离线状态（且自动恢复）")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_ESI_ERR)):
            print("接收的CAN FD报文的ESI标志置1")
        elif(BusStatus.Flags&(CANFD_DIAGNOSTIC_FLAG_DLC_MISMATCH)):
            print("DLC不匹配")
        else:
            print("无错误")

if __name__ == '__main__': 
    CAN1 = 0
    CAN2 = 1
    DevHandles = (c_uint * 20)()
    # Scan device
    ret = USB_ScanDevice(byref(DevHandles))
    if(ret == 0):
        print("No device connected!")
        exit()
    else:
        print("Have %d device connected!"%ret)
    # Open device
    ret = USB_OpenDevice(DevHandles[0])
    if(bool(ret)):
        print("Open device success!")
    else:
        print("Open device faild!")
        exit()
    # Get device infomation
    USB2XXXInfo = DEVICE_INFO()
    USB2XXXFunctionString = (c_char * 256)()
    ret = DEV_GetDeviceInfo(DevHandles[0],byref(USB2XXXInfo),byref(USB2XXXFunctionString))
    if(bool(ret)):
        print("USB2XXX device infomation:")
        print("--Firmware Name: %s"%bytes(USB2XXXInfo.FirmwareName).decode('ascii'))
        print("--Firmware Version: v%d.%d.%d"%((USB2XXXInfo.FirmwareVersion>>24)&0xFF,(USB2XXXInfo.FirmwareVersion>>16)&0xFF,USB2XXXInfo.FirmwareVersion&0xFFFF))
        print("--Hardware Version: v%d.%d.%d"%((USB2XXXInfo.HardwareVersion>>24)&0xFF,(USB2XXXInfo.HardwareVersion>>16)&0xFF,USB2XXXInfo.HardwareVersion&0xFFFF))
        print("--Build Date: %s"%bytes(USB2XXXInfo.BuildDate).decode('ascii'))
        print("--Serial Number: ",end='')
        for i in range(0, len(USB2XXXInfo.SerialNumber)):
            print("%08X"%USB2XXXInfo.SerialNumber[i],end='')
        print("")
        print("--Function String: %s"%bytes(USB2XXXFunctionString.value).decode('ascii'))
    else:
        print("Get device infomation faild!")
        exit()
    # 初始化CAN
    CANConfig = CANFD_INIT_CONFIG()
    # 获取波特率参数
    ret = CANFD_GetCANSpeedArg(DevHandles[0], byref(CANConfig), 500000, 2000000)
    if(ret != CANFD_SUCCESS):
        print("Get CAN1 Baud rate parameter failed!")
        exit()
    else:
        print("Get CAN1 Baud rate parameter Success!")
    #初始化CAN
    ret = CANFD_Init(DevHandles[0],CAN1,byref(CANConfig))
    if(ret != CANFD_SUCCESS):
        print("Config CAN1 failed!")
        exit()
    else:
        print("Config CAN1 Success!")

    ret = CANFD_Init(DevHandles[0],CAN2,byref(CANConfig))
    if(ret != CANFD_SUCCESS):
        print("Config CAN2 failed!")
        exit()
    else:
        print("Config CAN2 Success!")
    #设置过滤器，不设置默认接收所有数据
    FilterConfig = CANFD_FILTER_CONFIG()
    FilterConfig.Enable=1
    FilterConfig.Index=0
    FilterConfig.ID_Accept=0x123
    FilterConfig.ID_Mask=0xFFFFFFFF #对比所有位，也就是只接收ID为ID_Accept值的帧
    ret = CANFD_SetFilter(DevHandles[0],CAN2,byref(FilterConfig),1)
    if(ret != CANFD_SUCCESS):
        print("Config CAN2 Filter failed!")
        exit()
    else:
        print("Config CAN2 Filter Success!")
    # 复位时间戳
    DEV_ResetTimestamp(DevHandles[0])
    #获取初始化时的系统时间戳，并显示当前时间
    init_ts = datetime.datetime.now().timestamp()
    timeArray = time.localtime(init_ts)
    otherStyleTime = time.strftime("%Y-%m-%d %H:%M:%S", timeArray)
    print(otherStyleTime)
    # 启动CAN接收数据
    ret = CANFD_StartGetMsg(DevHandles[0],CAN2)
    if(ret != CANFD_SUCCESS):
        print("Start CAN2 failed!")
        exit()
    else:
        print("Start CAN2 Success!")
    # 发送CAN帧
    CanMsg = (CANFD_MSG*5)()
    for i in range(0,5):
        CanMsg[i].ID = i|CANFD_MSG_FLAG_IDE
        CanMsg[i].Flags = 0
        CanMsg[i].ID = 0x123+i
        CanMsg[i].DLC = 8
        CanMsg[i].TimeStamp = 10
        for j in range(0,CanMsg[i].DLC):
            CanMsg[i].Data[j] = (i<<4)|j
    SendedNum = CANFD_SendMsg(DevHandles[0],CAN1,byref(CanMsg),5)
    if SendedNum >= 0 :
        print("CAN1 success send frames:%d"%SendedNum)
    else:
        print("Send CAN1 data failed!")
    # Delay
    sleep(0.5)
    # 读取CAN数据
    CanMsgBuffer = (CANFD_MSG*10240)()
    CanNum = CANFD_GetMsg(DevHandles[0],CAN2,byref(CanMsgBuffer),10240)
    if CanNum > 0:
        print("CAN2 CanNum = %d"%CanNum)
        for i in range(0,CanNum):
            print("CanMsg[%d].ID = %d"%(i,CanMsgBuffer[i].ID))
            ts_ms = (CanMsgBuffer[i].TimeStamp+(CanMsgBuffer[i].TimeStampHigh<<32))/100;
            timeArray = time.localtime(init_ts+ts_ms/1000)
            otherStyleTime = time.strftime("%Y-%m-%d %H:%M:%S", timeArray)
            print("CanMsg[%d].TimeStamp = %f %s"%(i,init_ts+ts_ms/1000,otherStyleTime))
            print("CanMsg[%d].Data = "%i,end='')
            for j in range(0,CanMsgBuffer[i].DLC):
                print("%02X "%CanMsgBuffer[i].Data[j],end='')
            print("")
    elif CanNum == 0:
        print("No CAN2 data!")
    else:
        print("Get CAN2 data error!")
    # 停止CAN接收数据
    ret = CANFD_StopGetMsg(DevHandles[0],CAN2)
    if(ret != CANFD_SUCCESS):
        print("Stop CAN2 failed!")
        exit()
    else:
        print("Stop CAN2 Success!")
